#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>
#include "findCorners.h"
#include "Line.h"

#define DEBUG


using namespace cv;

int hueMin = 34;
int hueMax = 79;

int satMin = 133;
int satMax = 255;

int valMin = 63;
int valMax = 255;

int areaValue = 142;

int frameSizeX = 640;
int frameSizeY = 480;

CvMemStorage* contourStorage = NULL;

IplImage* frame = NULL;
IplImage* imgHSV = NULL;
IplImage* imgThreshed = NULL;

#ifdef DEBUG
CvScalar colorWhite = CV_RGB(255,255,255);
CvScalar colorRed = CV_RGB(255,0,0);
CvScalar colorGreen = CV_RGB(0, 255, 0);
CvScalar colorBlue = CV_RGB(0,0,255);
CvScalar colorYellow = CV_RGB(255,255,0);

#endif

CvPoint2D32f intersection_Array[500];
int intersection_Number = 0;

int boundingBoxSearchTolerance = 55;
CvPoint2D32f cornerArray[4];
CvPoint tempPoint = cvPoint(0,0);


void findStuff(int)
{
	if (contourStorage == NULL) //intitiaize stuff it it's not there already
	{
		contourStorage = cvCreateMemStorage(0);
		imgHSV = cvCreateImage(cvGetSize(frame), 8, 3);
		imgThreshed = cvCreateImage(cvGetSize(frame), 8, 1);
	}
	else
	{
		cvClearMemStorage(contourStorage);
	}

	CvSeq* contours = NULL;

	cvCvtColor(frame, imgHSV, CV_BGR2HSV);

	cvInRangeS(imgHSV, cvScalar(hueMin, satMin, valMin), cvScalar(hueMax, satMax, valMax), imgThreshed);
#ifdef DEBUG
	cvShowImage("Threshold Result", imgThreshed);
#endif
	//begin corner detection

	cvFindContours(imgThreshed, contourStorage, &contours);

	while (contours!=0)
	{

#ifdef DEBUG
		cvDrawContours(imgThreshed, contours, colorWhite, colorWhite, -1, 2, 8);
		//cvShowImage("Contourizable Crap", imgThreshed);
#endif

		double area = cvContourArea(contours);

		if (area > areaValue && !CV_IS_SEQ_HOLE(contours))
		{
			//get a series of points on the contour i the area inside the contour is greater than a threshold
			CvSeq* contourPoints = cvApproxPoly(contours, sizeof(CvContour), contourStorage, CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0); //sliderize the 0.02
			contourPoints = cvConvexHull2(contourPoints, 0, CV_CLOCKWISE, 1);
			CvSeq* contourPointsCopy = contourPoints;
			intersection_Number = 0;
			for (int i=0; i<contourPoints->total; i++)
			{
				CvPoint* tempPointPointer = (CvPoint*) (cvGetSeqElem(contourPoints, i));
				intersection_Number++;
				intersection_Array[i] = cvPointTo32f(cvPoint(tempPointPointer->x, tempPointPointer->y));
#ifdef DEBUG
				cvCircle(frame, cvPointFrom32f(intersection_Array[i]), 12, colorGreen, 2, 8, 0);
#endif
			}


			CvRect boundingRect = cvBoundingRect(contourPointsCopy, 1);
			RotatedRect minRect = cvMinAreaRect2(contourPointsCopy,NULL);
#ifdef DEBUG //draw bounding box and points on this current contour
			//cvRectangle(frame, cvPoint(boundingRect.x, boundingRect.y), cvPoint(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height), colorWhite, 1, 8, 0);
			Point2f rect_points[4]; minRect.points( rect_points );

			for (int j=0; j<4; j++)
			{
				cvLine(frame, rect_points[j], rect_points[(j+1)%4], colorRed, 1, 8);
			}
#endif


			//final point set for each contour at this point
			findIntersections(contourPoints, intersection_Array, &intersection_Number);
			filterIntersections(intersection_Array, &intersection_Number, boundingRect, boundingBoxSearchTolerance, cornerArray, frameSizeX);

			for (int i =0; i<intersection_Number; i++) //draw th intersection points
			{
				CvPoint currentPoint = cvPointFrom32f(intersection_Array[i]);
				if (currentPoint.x > 0 && currentPoint.x < frameSizeX && currentPoint.y > 0 && currentPoint.y < frameSizeY)
				{
					cvCircle(frame, currentPoint, 8, colorWhite, 2, 8, 0);
				}
			}

			//draw the colored points indicating corners
#ifdef DEBUG
			cvCircle(frame, cvPointFrom32f(cornerArray[0]), 2, colorRed, 2, 8, 0);
			cvCircle(frame, cvPointFrom32f(cornerArray[1]), 2, colorGreen, 2, 8, 0);
			cvCircle(frame, cvPointFrom32f(cornerArray[2]), 2, colorBlue, 2, 8, 0);
			cvCircle(frame, cvPointFrom32f(cornerArray[3]), 2, colorYellow, 2, 8, 0);
#endif



			//draw points on line
			//find the corners


		}
		contours = contours->h_next;
	}
#ifdef DEBUG
	cvShowImage("Polys", frame);
#endif
}


int main()
{
	CvCapture* capture = NULL;
#ifdef DEBUG
	cvNamedWindow("Input Image", 1);
	cvNamedWindow("Threshold Settings", 1);
	cvCreateTrackbar("Hue min", "Threshold Settings", &hueMin, 255, NULL);
	cvCreateTrackbar("Hue max", "Threshold Settings", &hueMax, 255, NULL);
	cvCreateTrackbar("Sat min", "Threshold Settings", &satMin, 255, NULL);
	cvCreateTrackbar("Sat max", "Threshold Settings", &satMax, 255, NULL);
	cvCreateTrackbar("Val min", "Threshold Settings", &valMin, 255, NULL);
	cvCreateTrackbar("Val max", "Threshold Settings", &valMax, 255, NULL);
	cvCreateTrackbar("Area threshold", "Threshold Settings", &areaValue, 5000, NULL);
	cvCreateTrackbar("Bounding Threshold", "Threshold Settings", &boundingBoxSearchTolerance, 100, NULL);
#endif





	//grab frame from camera
	capture = cvCaptureFromAVI("./IMG_022poop4.MOV"); //put file name here
	if (capture == NULL)
	{
		std::cout << "Capturing from camera.\n";
		capture = cvCaptureFromCAM(1);
		cvSetCaptureProperty(capture, CV_CAP_PROP_BRIGHTNESS, 0.00784); //2 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_CONTRAST, 0.2); //51 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_SATURATION, 0.498); //127 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_GAIN, 0); //0 in GUVC view
		cvSetCaptureProperty(capture, CV_CAP_PROP_AUTO_EXPOSURE, 0); //Have to set through GUVCview as 0, manual mode
		cvSetCaptureProperty(capture, CV_CAP_PROP_TEMPERATURE, 10000); //Have to set through GUVCview as 10000
		cvSetCaptureProperty(capture, CV_CAP_PROP_EXPOSURE, 166); //Have to set through GUVCview as 166
		cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, 224); //Have to set through GUVCview as 224
		//Backlight compensation 1, have to set through GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 640);
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 480);
	}

	frame = cvQueryFrame(capture);

	frameSizeX = cvGetSize(frame).width;
	frameSizeY = cvGetSize(frame).height;

	findStuff(1);
	cvWaitKey(0);

	while (true)
	{
		frame = cvQueryFrame(capture);
#ifdef DEBUG
		cvShowImage("Input Image", frame);
#endif

		if (!frame)
		{
			std::cout << "ERROR: Couldn't get a frames.\n";
		}
		double tick = getTickCount();
		findStuff(1);
		std::cout << "Time taken:" << (getTickCount() - tick) / getTickFrequency() * 1000 << " ms. \n";

		int c = cvWaitKey(2);
		if (c == 27)
		{
			break;
		}

	}
	cvReleaseImage(&frame);
}

